#include "ServoExt.h"

//#define usToTicks(_us)    ((clockCyclesPerMicrosecond() / 16 * _us) / 4)                 // converts microseconds to tick
//#define ticksToUs(_ticks) (((unsigned) _ticks * 16) / (clockCyclesPerMicrosecond() / 4))   // converts from ticks back to microseconds
//
//#define TRIM_DURATION  5                                   // compensation ticks to trim adjust for digitalWrite delays

//#define MIN_PULSE_WIDTH       544     // the shortest pulse sent to a servo
//#define MAX_PULSE_WIDTH      2400     // the longest pulse sent to a servo
//#define DEFAULT_PULSE_WIDTH  1500     // default pulse width when servo is attached

//#define MAX_PULSE_WIDTH      1800     // the longest pulse sent to a servo
//#define DEFAULT_PULSE_WIDTH  1000     // default pulse width when servo is attached

void ServoExt::ServoPIN(byte servo_pin)
{
  this->ServoPIN(servo_pin, 0);
}

void ServoExt::ServoPIN(byte servo_pin, int base)
{
  pinMode(servo_pin, OUTPUT);
    if(myServos[servo_pin] == NULL){
      myServos[servo_pin] = new Servo2();
    }
    if(!myServos[servo_pin]->attached()){
      myServos[servo_pin]->attach(servo_pin);
      myServosBase[servo_pin] = base;
      Serial.print("Init Servo pin: ");
      Serial.println(servo_pin);
    }
}

void ServoExt::SetServoDegree(byte servo_pin, int angle)
{
  this->SetServoDegree(servo_pin, angle, 0);
}
void ServoExt::SetServoDegree(byte servo_pin, int servo_degree, int delaySeconds)
{
  this->ServoPIN(servo_pin);

  // 控制方式1
  //    int Degree = Angle;
  //    int servo_degree;
  //    if (Degree > 360) {
  //      return;
  //    }
  //    if (Degree == 90 || Degree == 270) {
  //      servo_degree = ServoBaseDegree;
  //    } else if (Degree >= 0 && Degree <= 180) {
  //      servo_degree = ServoBaseDegree - 90 + Degree;   // 180-degree-diff
  //    }

  //    int pulsewidth = servo_degree - TRIM_DURATION;
  //    pulsewidth = usToTicks(pulsewidth);  // convert to ticks after compensating for interrupt overhead

  //int val = map(servo_degree, 0, 90, MIN_PULSE_WIDTH, DEFAULT_PULSE_WIDTH);
  //Serial.print("map val: ");
  //Serial.println(val);
  //  int pulsewidth = (servo_degree * 11) + 510;

// 控制方式2
//  int pulsewidth;
//  if (servo_degree <= 90) {
//    pulsewidth =  map(servo_degree, 0, 90, MIN_PULSE_WIDTH, DEFAULT_PULSE_WIDTH);
//  } else {
//    pulsewidth =  map(servo_degree, 90, 180, DEFAULT_PULSE_WIDTH, MAX_PULSE_WIDTH);
//  }
//
//  Serial.print("degree val: ");
//  Serial.print(servo_degree);
//  Serial.print(", pulsewidth val: ");
//  Serial.println(pulsewidth);
//
//  long ccpm = clockCyclesPerMicrosecond();
//  Serial.print("clockCyclesPerMicrosecond val: ");
//  Serial.println(ccpm);
//
//  for (int i = 0; i < 180; i++) {
//    digitalWrite(servo_pin, HIGH);   //Set the servo interface level to high
//    delayMicroseconds(pulsewidth);  //The number of microseconds of the delay pulse width value
//    digitalWrite(servo_pin, LOW);    //Set the servo interface level to low
//    delayMicroseconds(20000 - pulsewidth);
//  }
  //  delay(250);

// 控制方式3
  	myServos[servo_pin]->write(servo_degree);
  if (delaySeconds > 0) {
    delay(delaySeconds);
    myServos[servo_pin]->detach();
  }
  // for (int i = 0; i < 16; i++) {
  //    if(myServos[i] == NULL){
  //      Serial.print("not init: ");
  //      Serial.println(i);
  //    }
  // }
}
void ServoExt::detach()
{
     for (int i = 0; i < 16; i++) {
      if(myServos[i] != NULL && myServos[i]->attached()){
        myServos[i]->detach();
      }
   }
}

void ServoExt::detach(int delaySeconds) {
  if (delaySeconds > 0) {
    delay(delaySeconds);
  }
  this->detach();
}
